#include "special_scene.h"
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include <map>
#include "io_macro.h"
#define LIGHT_ALRM_TIME_OUT 3000 // 灯光启动时，3s报警
namespace io_module {

SpecialScene::SpecialScene(const std::shared_ptr<rclcpp::Node>& node):
    m_trigIdleTimer(node),
    m_trigCurveTimer(node)
{
    m_curLight = NoControl;
    m_lastStatus.idle = m_curStatus.idle = true;
}

// 必须先更新，在调用getScene 否则差一个周期相应
void SpecialScene::updateStatus(agv_msgs::msg::AgvAutoStatus inStatus)
{
    m_curStatus = inStatus;
    // 如果两种情况同时发生，则圆弧场景覆盖启动报警
    //   #1 上一次空闲，当前不是空闲
    //   触发一次定时器
    if (m_lastStatus.idle && !m_curStatus.idle)
    {
        m_trigIdleTimer.reset();
        m_trigIdleTimer.start();
        m_curLight = ReadyStartLight;
        DEBUG_STREAM_OUT("ReadyStartLight once");
    }

    // #2 圆弧起点
    bool lastCurve = m_lastStatus.trajtype == agv_msgs::msg::AgvAutoStatus::TRAJ_BEZIER;
    bool curCurve = m_curStatus.trajtype == agv_msgs::msg::AgvAutoStatus::TRAJ_BEZIER;
    if (!lastCurve && curCurve)
    {
        m_trigCurveTimer.reset();
        m_trigCurveTimer.start();
        m_curLight = ReadyCurveLight;
        DEBUG_STREAM_OUT("ReadyCurveLight once");
    }

    m_lastStatus = m_curStatus;
}

int SpecialScene::getScene()
{

    switch (m_curLight)
    {
    case ReadyStartLight:
    {
        if (m_trigIdleTimer.check_continuous(LIGHT_ALRM_TIME_OUT))
        {
            m_curLight = NoControl;
            DEBUG_STREAM_OUT("getScene: set m_curLight ReadyStartLight-> NoControl");
        }
    }
    break;

    case ReadyCurveLight:
    {
        if (m_trigCurveTimer.check_continuous(LIGHT_ALRM_TIME_OUT))
        {
            m_curLight = NoControl;
            DEBUG_STREAM_OUT("getScene: set m_curLight ReadyCurveLight-> NoControl");
        }
    }
    break;
    default:
    {
        // NoControl or CurveLight
        //  上一次圆弧bit位未触发，当前bit位触发
        bool curCurveBit = TestBit(m_curStatus.bits, CURVE_LIGHT_BITS);
        if (curCurveBit)
        {
            m_curLight = CurveLight;
            DEBUG_STREAM_OUT("getScene: set m_curLight CurveLight");
        }
        else
        {
            m_curLight = NoControl;
            static int time = 0;
            if (time++ > 10)
            {
                time = 0;
                DEBUG_STREAM_OUT("getScene: set m_curLight NoControl");
            }
        }
    }

    break;
    }

    return m_curLight;
}
}